import os
import sys
import math
BASE_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.append(BASE_DIR)

from Source.CWJ import qyhf
from Methods.tools import *
from Source.HJL import PipeClass
from Source.HJL import BlockClass
from Method import blocks_select
from Avoidance_CURD import Pi_bypass, pilist_avoidance, Set_slope, LinkedList
from secondary_main import *

#含三通风管排管算法
'''
需要的输入:
pipeBase:管道信息(类的形式)
blockBase:障碍物信息(类的形式)
block_arr:障碍物信息(数组形式)
PipeGroupNo:管道组的序号
new_allpipelist:所有避障后的新管道数组,基本类型为[列,列,列...]
plist_flag:避障算法标记位,决定该组每列管道的避障算法,0为逻辑算法,1为A*,长度为列的长度
'''
def Duct_Tee_Sorting(pipeBase,blockBase,block_arr,PipeGroup_info,new_allpipelist,plist_flag):
    PipeGroupNo = PipeGroup_info[0]
    PipeGroup_Type = PipeGroup_info[1]
    ceilling = PipeGroup_info[2]
    # 获取三通分类,确定排管顺序,start_point为含起点三通,other_point为其他三通
    for PipeGroup in pipeBase._pipeGroups:
        if PipeGroup.pipeGroupNo == PipeGroupNo:
            _, start_point, other_point = PipeGroup.getTeeType()
            break

    # print(start_point)
    # 寻找开始列管道标号start_label
    start_label = find_startlabel(pipeBase,PipeGroupNo)

    # 持续遍历起点三通队列，只要队列不为空，则不跳出循环
    while start_point !=[]:

        start_tee = start_point.pop(0)# 取起点三通队列的第一个三通对象
        Lists = start_tee.lists#当前三通的三列标号,list类型,即[1,3,2]
        t = len(Lists)

        #寻找起点列的标号s_pipe_label,终点列标号e_pipe_label
        e_pipe_label = []
        for i in Lists:
            if Get_pipelist_mark(PipeGroupNo, i, pipeBase) == 'S':
                s_pipe_label = i
            else:
                e_pipe_label.append(i)

        #纠正三列管道的坐标
        s_pipe_arr = PipeListNo_to_PipeList(PipeGroupNo,s_pipe_label,pipeBase)#s列管道数组

        e_pipe_arr = [PipeListNo_to_PipeList(PipeGroupNo,e_pipe_label[t],pipeBase) for t in range(0,t-1)]#两个e列管道数组

        e_pipe_arr = list_E_reverse(e_pipe_arr,length = 4000)#先纠正两个终点列坐标

        #如果起点列是第一个三通的S列,则排管
        if s_pipe_label == start_label:
            s_pipe_arr = list_1_S_reverse(s_pipe_arr, e_pipe_arr[0], length=4000)
            flag_arr = [plist_flag[start_label-1],0,1]#此为标记数组，第一位为避障算法标记位，0为逻辑算法，1为A*;第二、三位为管道列起点、终点标记位，0表示不可以移动，1表示可以移动
            #排S列管道
            print('------------------正在排第', start_label, '号管道,S列管道-----------------------')
            change_flag, slope_change_flag, s_pipe_arr, position_list = duct_pipelist_main(flag_arr, s_pipe_arr, block_arr, blockBase,PipeGroup_Type, ceilling, pipeBase)
            double_update(s_pipe_arr, pipeBase, PipeGroupNo, start_label, position_list, change_flag, slope_change_flag)#将管道数组信息更新至管道类
            new_allpipelist.append(s_pipe_arr)

        #将两个E列的起点坐标改成S列的终点坐标,两个E列所有段的高度改成S列的高度,并且需要调整为竖直或水平方向
        e_pipe_correction(e_pipe_arr, s_pipe_arr, change_flag, slope_change_flag)
        # e_pipe_arr[0][0][0],e_pipe_arr[1][0][0] = s_pipe_arr[-1][1],s_pipe_arr[-1][1]
        # duct_change_height(e_pipe_arr, s_pipe_arr[-1][1][2])


        #判断管道的大小，决定排管顺序,这里直接调整e_pipe_label, e_pipe_arr的排列顺序,大的在前，小的在后

        e_pipe_label, e_pipe_arr = size_judge(e_pipe_label, e_pipe_arr)
        tee_block_flag = [1]  # 三通管道障碍物标记位,用于判断是否将当前列管道添加到障碍物列表,0表示不添加,1表示添加,起点管一定要添加,所以先置1
        #排两个E列大小管道,e_pipe_label,e_pipe_arr;判断是否命中三通,如果命中,则将该列mark标记为S,并将命中三通升级为起点三通
        for k in range(0, t-1):

            Tee_object = Tee_judge(e_pipe_label[k], start_point, other_point)
            if Tee_object != None:#说明命中了三通
                tee_block_flag.append(0)
                Change_pipelist_mark(PipeGroupNo,e_pipe_label[k], pipeBase)#改变mark
                duct_Tee_upgrade(Tee_object, start_point, other_point)#三通升级
                flag_arr = [plist_flag[e_pipe_label[k]-1], 0, 1]#如果该列命中了下一个三通,那么该列的起点不可移动,终点可以微调

                print('------------------正在排第', e_pipe_label[k], '号管道，连接另一个三通-----------------------')
                change_flag, slope_change_flag, e_pipe_arr[k], position_list = duct_pipelist_main(flag_arr, e_pipe_arr[k], block_arr, blockBase,PipeGroup_Type, ceilling, pipeBase)#排管
                double_update(e_pipe_arr[k], pipeBase, PipeGroupNo, e_pipe_label[k], position_list, change_flag, slope_change_flag)#将管道数组信息更新至管道类
                new_allpipelist.append(e_pipe_arr[k])
            else:
                tee_block_flag.append(1)
                flag_arr = [plist_flag[e_pipe_label[k]-1], 0, 0]
                print('------------------正在排第', e_pipe_label[k], '号管道，E列管道-----------------------')
                change_flag, slope_change_flag, e_pipe_arr[k], position_list = duct_pipelist_main(flag_arr, e_pipe_arr[k], block_arr, blockBase,PipeGroup_Type, ceilling, pipeBase)
                double_update(e_pipe_arr[k], pipeBase, PipeGroupNo, e_pipe_label[k], position_list, change_flag, slope_change_flag)#将管道数组信息更新至管道类
                new_allpipelist.append(e_pipe_arr[k])

        # 排完当前三通的三列管道后,把起点和终点列添加到障碍物列表,命中三通的列不添加
        # tee_block = [s_pipe_arr,e_pipe_arr[0],e_pipe_arr[1]]
        #存储新生成的管道列,分别为起点管,大风管,小风管
        tee_block =[s_pipe_arr]
        for i in range(0,t-1):
            tee_block.append(e_pipe_arr[i])

        for j in range(0,t):
            if tee_block_flag[j] == 1:
                try:
                    new_blocklist = qyhf.pipes_to_blocks(tee_block[j], 10)
                    # # 管道类型的障碍物，添加到障碍物对象中
                    add(new_blocklist, blockBase)
                    # # 管道类型的障碍物，添加到障碍物数组中
                    block_arr.extend(new_blocklist)
                except:
                    print('qyhf.pipes_to_blocks跑不通，新管道无法变成障碍物')
# #获取风管三通的分类,及起点通和其他通
# def getTee(pipeBase,PipeGroupNo):
#     for PipeGroup in pipeBase._pipeGroups:
#         if PipeGroup.pipeGroupNo == PipeGroupNo:
#             _, start_point, other_point = PipeGroup.getTeeType()
#     return start_point,other_point
#输入当前E列标号label，判断是否命中三通，如果命中，则返回命中的三通对象，否则返回None
def Tee_judge(label,start_point,other_point):
    Tee_object = None
    for tee in other_point:
        tee_labels = tee.lists#三通的三列标号
        for i in tee_labels:
            if label == i:
                Tee_object = tee
                break
    return Tee_object
#三通升级函数,输入待升级的对象,对两个队列做出改变
def duct_Tee_upgrade(Tee_object,start_point,other_point):
    if Tee_object in other_point:
        other_point.remove(Tee_object)
        start_point.append(Tee_object)
    else:
        print('三通升级函数出错,返回原栈！')

#由管道列标号,返回该列管道数组
def PipeListNo_to_PipeList(PipeGroupNo,PipeListNo,pipeBase):
    a = []
    for PipeGroup in pipeBase._pipeGroups:
        # print(PipeGroup.pipeGroupNo)
        if PipeGroup.pipeGroupNo == PipeGroupNo:
            for pipeList in PipeGroup._pipeLists:
                if pipeList.pipeListNo == PipeListNo:
                    for pipe in pipeList._pipes:
                        # todo 2023-12-7添加 if_successful_avoid
                        a.append([pipe.start, pipe.end, pipe.size.width, pipe.size.height, pipe.angle, pipe.if_successful_avoid])
    return a

#判断两段管是否离得很近,利用[x,y]来判断,这里是输入为长度为3的数组,即[x,y,z]
def coincide_judge(pipe1,pipe2,length=4000):
    if math.sqrt((pipe1[0]-pipe2[0])**2+(pipe1[1]-pipe2[1])**2) < length:
        return True
    else:
        return False
#将一列坐标的起点和终点互换
def pipelist_reverse(plist):
    length = len(plist)
    for i in plist:
        temp = i[0]
        i[0] = i[1]
        i[1] = temp
    a = []
    for j in range(length-1,-1,-1):
        a.append(plist[j])
    return a
#纠正两列E管道的起点和终点坐标
def list_2_E_reverse(e_pipe_arr,length = 4000):
    plist1,plist2 = e_pipe_arr[0],e_pipe_arr[1]
    if coincide_judge(plist1[0][0], plist2[0][0], length) == True:#如果两管道起点坐标靠近,则返回原管道,这里比较的是起点坐标的x，y
        return [plist1,plist2]
    else:
        plist1 = pipelist_reverse(plist1)#否则把plist1 反向再比较
        if coincide_judge(plist1[0][0], plist2[0][0], length) == True:
            return [plist1, plist2]
        else:
            plist1 = pipelist_reverse(plist1)#否则把plist2 S和E 坐标互换再比较
            plist2 = pipelist_reverse(plist2)
            if coincide_judge(plist1[0][0], plist2[0][0], length) == True:
                return [plist1, plist2]
            else:
                plist1 = pipelist_reverse(plist1)#否则把plist2,plist2 S和E 坐标互换再比较
                if coincide_judge(plist1[0][0], plist2[0][0], length) == True:
                    return [plist1, plist2]
                else:
                    plist1 = pipelist_reverse(plist1)
                    plist2 = pipelist_reverse(plist2)
                    print('互换坐标函数出错，返回原管道')
                    return [plist1, plist2]
#纠正两/三列E管道的起点和终点坐标
def list_E_reverse(e_pipe_arr , length = 4000):
    if len(e_pipe_arr) == 2:
        return list_2_E_reverse(e_pipe_arr, length)
    elif len(e_pipe_arr) == 3:
        plist1 = list_2_E_reverse(e_pipe_arr[0:2], length)
        plist2 = list_2_E_reverse(e_pipe_arr[1:3], length)
        return [plist1[0],plist1[1],plist2[1]]
    else:
        print('纠正终点管道坐标出错，返回原管道')
        return e_pipe_arr

#纠正S列管道的起点和终点坐标
def list_1_S_reverse(s_plist,e_plist,length = 4000):
    if coincide_judge(s_plist[-1][1], e_plist[0][0], length) == True:#如果S管道终点和E管道起点靠近,则返回原管道,这里比较的是起点坐标的x,y
        return s_plist
    else:
        s_plist = pipelist_reverse(s_plist)#否则把s_plist 反向再比较
        if coincide_judge(s_plist[-1][1], e_plist[0][0], length) == True:
            return s_plist
        else:
            print('互换坐标函数出错，返回原管道')
            return s_plist

# #改变E列start和end高度,数组中改正,直接排管
# def duct_change_height(e_pipe_arr,height):
#     for plist in e_pipe_arr:
#         for pipe in plist:
#             pipe[0][2]=height
#             pipe[1][2]=height

# 获取当前列的mark,判断其是否是起点S
def Get_pipelist_mark(PipeGroupNo, PipeListNo, pipeBase):
    for PipeGroup in pipeBase._pipeGroups:
        if PipeGroup.pipeGroupNo == PipeGroupNo:
            for PipeList in PipeGroup._pipeLists:
                if PipeList.pipeListNo == PipeListNo:
                    return PipeList.mark
                    break
# 改变命中三通列管道mark函数
def Change_pipelist_mark(PipeGroupNo, PipeListNo, pipeBase):
    for PipeGroup in pipeBase._pipeGroups:
        if PipeGroup.pipeGroupNo == PipeGroupNo:
            for PipeList in PipeGroup._pipeLists:
                if PipeList.pipeListNo == PipeListNo:
                    PipeList.mark = 'S'
                    break

# #输入pipebase,PipeGroupNo寻找起点列标号
# def find_startlabel(pipeBase,PipeGroupNo):
#     startlabel = -1
#     # print('这里！！！')
#     for pipeList in pipeBase._pipeGroups[PipeGroupNo-1]._pipeLists:#pipeList为对象类型
#         if pipeList.mark == 'S':
#             startlabel = pipeList.pipeListNo
#         else:
#             continue
#     if startlabel == -1:
#         print('寻找起点列坐标出错，返回值为-1')
#     return startlabel
#输入pipebase,PipeGroupNo寻找起点列标号
def find_startlabel(pipeBase,PipeGroupNo):
    startlabel = -1
    # print('这里！！！')
    for PipeGroup in pipeBase._pipeGroups:
        if PipeGroup.pipeGroupNo == PipeGroupNo:
            for PipeList in PipeGroup._pipeLists:
                if PipeList.mark == 'S':
                    startlabel = PipeList.pipeListNo
                    print('mark',startlabel)
                else:
                    continue
    if startlabel == -1:
        print('寻找起点列坐标出错，返回值为-1')
    return startlabel
#判断水管尺寸,决定排管顺序,输入两个E列管道数组e_pipe_arr及其对应标号e_pipe_label,返回大小管道标号e_pipe_label,数组e_pipe_arr
def size_judge(e_pipe_label,e_pipe_arr):
    if len(e_pipe_label) == 2:
        if e_pipe_arr[0][0][2]*e_pipe_arr[0][0][3] < e_pipe_arr[1][0][2]*e_pipe_arr[1][0][3]:
            e_pipe_label.reverse()
            e_pipe_arr.reverse()
            return e_pipe_label,e_pipe_arr
        else:
            return e_pipe_label,e_pipe_arr
    elif len(e_pipe_label) == 3:
        if e_pipe_arr[0][0][2]*e_pipe_arr[0][0][3] < e_pipe_arr[1][0][2]*e_pipe_arr[1][0][3]:
            temp_a,temp_l = e_pipe_arr[1],e_pipe_label[1]
            e_pipe_arr[1],e_pipe_label[1] = e_pipe_arr[0],e_pipe_label[0]
            e_pipe_arr[0],e_pipe_label[0] = temp_a,temp_l
        if e_pipe_arr[0][0][2]*e_pipe_arr[0][0][3] < e_pipe_arr[2][0][2]*e_pipe_arr[2][0][3]:
            temp_a,temp_l = e_pipe_arr[2],e_pipe_label[2]
            e_pipe_arr[2],e_pipe_label[2] = e_pipe_arr[0],e_pipe_label[0]
            e_pipe_arr[0],e_pipe_label[0] = temp_a,temp_l
        if e_pipe_arr[1][0][2]*e_pipe_arr[1][0][3] < e_pipe_arr[2][0][2]*e_pipe_arr[2][0][3]:
            temp_a,temp_l = e_pipe_arr[2],e_pipe_label[2]
            e_pipe_arr[2],e_pipe_label[2] = e_pipe_arr[1],e_pipe_label[1]
            e_pipe_arr[1],e_pipe_label[1] = temp_a,temp_l
        return e_pipe_label,e_pipe_arr
    else:
        print('管道大小判断函数出错,返回原管道')
        return e_pipe_label, e_pipe_arr

#判断风管的方向,输入当前列管道,返回方向,x表示水平,y表示垂直
def direction_judge(plist):
    if abs(plist[0][0][0] - plist[0][1][0]) < 1:#表示起点和终点的x坐标一样,这里之所以这样判断是因为存在极小误差
        return 'y' #表示管道沿着y轴方向
    elif abs(plist[0][0][1] - plist[0][1][1]) < 1:#表示起点和终点的y坐标一样
        return 'x'
    else:
        print('风管方向判断出错，返回空')
        return None


# 将两个E列的起点坐标改成S列的终点坐标,两个E列所有段的高度改成S列的高度,并且需要调整为竖直或水平方向
def e_pipe_correction(e_pipe_arr,s_pipe_arr, change_flag, slope_change_flag):
    xs = s_pipe_arr[-1][1][0]#S列终点的x坐标
    ys = s_pipe_arr[-1][1][1]#S列终点的y坐标
    if change_flag or slope_change_flag:
        for i in range(len(e_pipe_arr)):
            if direction_judge(e_pipe_arr[i]) == 'x':
                e_pipe_arr[i][0][1][1] = ys  # 将E列管道第一段的终点y坐标改成和S列终点的y坐标一致
                e_pipe_arr[i][0][0] = s_pipe_arr[-1][1]  # 将E列管道起点坐标调整为S列终点
                # if len(e_pipe_arr[i]) > 1:#如果当前列有多段,则第二段坐标需要调整
                #     e_pipe_arr[i][1][0] = e_pipe_arr[i][0][1]#第二段的起点坐标等于第一段终点坐标
                if len(e_pipe_arr[i]) > 1:  # 如果当前列有多段,则第二段坐标需要调整

                    for j in range(1, len(e_pipe_arr[i])):
                        if direction_judge([e_pipe_arr[i][j]]) == 'x':
                            e_pipe_arr[i][j][1][1] = e_pipe_arr[i][j - 1][1][1]  # 将E列管道第一段的终点y坐标改成和S列终点的y坐标一致
                            e_pipe_arr[i][j][0] = e_pipe_arr[i][j - 1][1]  # 将E列管道起点坐标调整为S列终点
                        else:
                            e_pipe_arr[i][j][1][0] = e_pipe_arr[i][j - 1][1][0]  # 将E列管道后段的终点x坐标改成和S列终点的x坐标一致
                            e_pipe_arr[i][j][0] = e_pipe_arr[i][j - 1][1]  # 将E列管道后段起点坐标调整为前段终点坐标
            elif direction_judge(e_pipe_arr[i]) == 'y':
                e_pipe_arr[i][0][1][0] = xs  # 将E列管道第一段的终点x坐标改成和S列终点的x坐标一致
                e_pipe_arr[i][0][0] = s_pipe_arr[-1][1]
                # if len(e_pipe_arr[i]) > 1:#如果当前列有多段,则第二段坐标需要调整
                #     e_pipe_arr[i][1][0] = e_pipe_arr[i][0][1]
                if len(e_pipe_arr[i]) > 1:  # 如果当前列有多段,则后面各段也需要调整
                    for j in range(1, len(e_pipe_arr[i])):
                        if direction_judge([e_pipe_arr[i][j]]) == 'x':
                            e_pipe_arr[i][j][1][1] = e_pipe_arr[i][j - 1][1][1]  # 将E列管道第一段的终点y坐标改成和S列终点的y坐标一致
                            e_pipe_arr[i][j][0] = e_pipe_arr[i][j - 1][1]  # 将E列管道起点坐标调整为S列终点
                        else:
                            e_pipe_arr[i][j][1][0] = e_pipe_arr[i][j - 1][1][0]  # 将E列管道后段的终点x坐标改成和S列终点的x坐标一致
                            e_pipe_arr[i][j][0] = e_pipe_arr[i][j - 1][1]  # 将E列管道后段起点坐标调整为前段终点坐标
            else:
                print('E列管道校正出错,返回原坐标')
            # 调整E列管道的所有Z坐标
            for pipe in e_pipe_arr[i]:
                pipe[0][2] = s_pipe_arr[-1][1][2]
                pipe[1][2] = s_pipe_arr[-1][1][2]
    else:
        print("原S管道没有发生变化，不用修改E坐标")





